function [J,Jd] = jacobian(theta,theta_dot)

% parameters according to human body
height = 1.5; % [m]
L1 = 0.285*height; % [m]
L2 = (0.53 - 0.285)*height; % [m]
L3 = height - (L1 + L2); % [m]
L4 = L2; % [m]
L5 =L1; % [m]

r1 = L1/2; % [m]
r2 = L2/2; % [m]
r3 = L3/2; % [m]    
r4 = L4/2; % [m]
r5 = L5/2; % [m]

theta1 = theta(1);
theta2 = theta(2);
theta3 = theta(3);
theta4 = theta(4);
theta5 = theta(5);

theta1_dot = theta_dot(1);
theta2_dot = theta_dot(2);
theta3_dot = theta_dot(3);
theta4_dot = theta_dot(4);
theta5_dot = theta_dot(5);

J=[ 
                                                                                                                                   cos(theta1)*(L1 - r1),                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                  -sin(theta1)*(L1 - r1),                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                  cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1),                                                                                                  cos(theta1 + theta2)*(L2 - r2),                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1),                                                                                                 -sin(theta1 + theta2)*(L2 - r2),                                                                                                0,                                                                                                0,                                                   0; ...
                                                               cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3),                                                               cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3),                                                                 r3*cos(theta1 + theta2 + theta3),                                                                                                0,                                                   0; ...
                                                             - r3*sin(theta1 + theta2 + theta3) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1),                                                             - r3*sin(theta1 + theta2 + theta3) - sin(theta1 + theta2)*(L2 - r2),                                                                -r3*sin(theta1 + theta2 + theta3),                                                                                                0,                                                   0; ...
                                                      cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4),                                                      cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4),                                                       -r4*cos(theta1 + theta2 + theta3 - theta4),                                                        r4*cos(theta1 + theta2 + theta3 - theta4),                                                   0; ...
                                                      r4*sin(theta1 + theta2 + theta3 - theta4) - sin(theta1)*(L1 - r1) - sin(theta1 + theta2)*(L2 - r2),                                                      r4*sin(theta1 + theta2 + theta3 - theta4) - sin(theta1 + theta2)*(L2 - r2),                                                        r4*sin(theta1 + theta2 + theta3 - theta4),                                                       -r4*sin(theta1 + theta2 + theta3 - theta4),                                                   0; ...
 cos(theta1 + theta2)*(L2 - r2) - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4), cos(theta1 + theta2)*(L2 - r2) - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r4*cos(theta1 + theta2 + theta3 - theta4), - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r4*cos(theta1 + theta2 + theta3 - theta4),   r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4), -r5*cos(theta1 + theta2 + theta3 - theta4 + theta5); ...
 r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4), r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4),   r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4), - r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - r4*sin(theta1 + theta2 + theta3 - theta4),  r5*sin(theta1 + theta2 + theta3 - theta4 + theta5); ...
                                                                                                                                                      -1,                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                1,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                1,                                                  -1];
 


Jd =[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -theta1_dot*sin(theta1)*(L1 - r1),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0; ...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -theta1_dot*cos(theta1)*(L1 - r1),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) - theta2_dot*sin(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 - theta1_dot*sin(theta1 + theta2)*(L2 - r2) - theta2_dot*sin(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1)) - theta2_dot*cos(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 - theta1_dot*cos(theta1 + theta2)*(L2 - r2) - theta2_dot*cos(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) - theta2_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - theta2_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                             - r3*theta1_dot*sin(theta1 + theta2 + theta3) - r3*theta2_dot*sin(theta1 + theta2 + theta3) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3)) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                             - r3*theta1_dot*cos(theta1 + theta2 + theta3) - r3*theta2_dot*cos(theta1 + theta2 + theta3) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                         r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - theta2_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                         r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - theta2_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta1_dot*sin(theta1 + theta2 + theta3 - theta4) + r4*theta2_dot*sin(theta1 + theta2 + theta3 - theta4) + r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta2_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta1_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                         r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                         r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta1_dot*cos(theta1 + theta2 + theta3 - theta4) + r4*theta2_dot*cos(theta1 + theta2 + theta3 - theta4) + r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta2_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta1_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                                                             0;...
 theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), r5*theta1_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta2_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta3_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) - r5*theta4_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5);...
 theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) - cos(theta1)*(L1 - r1) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), r5*theta1_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta2_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta3_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r5*theta4_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5);...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0];
 

% syms theta1 theta2 theta3 theta4 theta5 theta1_dot theta2_dot theta3_dot theta4_dot theta5_dot theta1_acc theta2_acc theta3_acc theta4_acc theta5_acc
% syms u1 u2 u3 u4 u5 % forces and moments
% syms m1 m2 m3 m4 m5 r1 r2 r3 r4 r5 L1 L2 L3 L4 L5
% 
% % parameters according to human body
% height = 1.5; % [m]
% L1 = 0.285*height; % [m]
% L2 = (0.53 - 0.285)*height; % [m]
% L3 = height - (L1 + L2); % [m]
% L4 = L2; % [m]
% L5 =L1; % [m]
% 
% r1 = L1/2; % [m]
% r2 = L2/2; % [m]
% r3 = L3/2; % [m]    
% r4 = L4/2; % [m]
% r5 = L5/2; % [m]
% 
% % Vector of generalized coordinates
% q = [theta1; theta2; theta3; theta4; theta5];
% 
% % Vector of generalized velocities
% qd = [theta1_dot; theta2_dot; theta3_dot; theta4_dot; theta5_dot];
% 
% % orientations
% o1 =  -q(1);
% o2 =  -q(1) - q(2);
% o3 = -q(1) - q(2) - q(3);
% o4 = -q(1) - q(2) - q(3) + q(4);
% o5 = -q(1) - q(2) -q(3) +q(4) - q(5);
% o =  [o1 o2 o3 o4 o5];
% 
% % position and orientation vector
% r(1,1) = (-L1 + r1) * sin(o1); % x cm position of link-1
% r(2,1) = (L1 - r1)* cos(o1); % y cm position of link-1
% r(3,1) = r(1,1) + (-L2 + r2) * sin(o2);% x cm position of link-2
% r(4,1) = r(2,1) + (L2-r2) *cos(o2); % y cm position of link-2
% r(5,1) = r(3,1) - r3 * sin(o3);% x cm position of link-3
% r(6,1) = r(4,1) + r3 * cos(o3);  % y cm position of link-3
% r(7,1) = r(3,1) + r4 * sin(o4); % x cm position of link-4
% r(8,1) = r(4,1) - r4 * cos(o4); % y cm position of link-4
% r(9,1) = r(7,1) + r5 * sin(o5); % x cm position of link-5
% r(10,1) = r(8,1)- r5 * cos(o5); % y cm position of link-5
% r(11,1) = o(1); % orientation of link-1 
% r(12,1) = o(2); % orientation of link-2 
% r(13,1) = o(3); % orientation of link-3 
% r(14,1) = o(4); % orientation of link-4 
% r(15,1) = o(5); % orientation of link-5 
% 
% % Jacobian
% J = [diff(r, theta1), diff(r, theta2), diff(r,theta3), diff(r,theta4), diff(r,theta5)];
% 
% % Derivative of Jacobian in time
% for j = 1:length(J(1,:))
%         Jd(:,j) = [diff(J(:,j),q(1)), diff(J(:,j),q(2)), diff(J(:,j),q(3)),diff(J(:,j),q(4)), diff(J(:,j),q(5))]*qd;
% end
% 
% theta1 = theta(1);
% theta2 = theta(2);
% theta3 = theta(3);
% theta4 = theta(4);
% theta5 = theta(5);
% 
% theta1_dot = theta_dot(1);
% theta2_dot = theta_dot(2);
% theta3_dot = theta_dot(3);
% theta4_dot = theta_dot(4);
% theta5_dot = theta_dot(5);
% 
% J=eval(J);
% Jd=eval(Jd);

end